#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include <vector>

using std::placeholders::_1;
using std::string;

class DataSynchronizerAndTfPublisher : public rclcpp::Node
{
private:
    // TF 广播器
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;

    // 原始话题的订阅者
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;

    // 时间戳同步后的话题发布者
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;

public:
    explicit DataSynchronizerAndTfPublisher(const string &name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "节点 %s: 已启动. [分离式回调 + 安全余量模式]", name.c_str());

        // --- 初始化参数和TF广播器 ---
        if (!this->has_parameter("use_sim_time")) {
            this->declare_parameter<bool>("use_sim_time", true);
        }
        RCLCPP_INFO(this->get_logger(), "是否使用模拟时间: %s", this->get_parameter("use_sim_time").as_bool() ? "是" : "否");
        
        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
        static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
        
        publish_static_tfs_from_urdf();

        // --- 初始化所有发布者 ---
        odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/dog_odom_synced", 10);
        imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("/dog_imu_raw_synced", 10);
        pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/rslidar_points_synced", 10);

        // --- 初始化所有订阅者 (使用分离的回调函数) ---
        odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "/dog_odom", 10, std::bind(&DataSynchronizerAndTfPublisher::odom_callback, this, _1));
        
        imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
            "/dog_imu_raw", 10, std::bind(&DataSynchronizerAndTfPublisher::imu_callback, this, _1));

        pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "/rslidar_points", 10, std::bind(&DataSynchronizerAndTfPublisher::pointcloud_callback, this, _1));
        
        RCLCPP_INFO(this->get_logger(), "已订阅所有原始话题，准备进行同步和处理...");
    }

private:
    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        rclcpp::Time now = this->now();

        // 1. 高频发布TF
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = now;
        transform.header.frame_id = "odom";
        transform.child_frame_id = "base_link";
        transform.transform.translation.x = msg->pose.pose.position.x;
        transform.transform.translation.y = msg->pose.pose.position.y;
        // transform.transform.translation.z = msg->pose.pose.position.z;
        // transform.transform.rotation = msg->pose.pose.orientation;
        // tf_broadcaster_->sendTransform(transform);
        // msg->header.stamp = now;
        // odom_pub_->publish(*msg);
        transform.transform.translation.z = 0.0; 
        transform.transform.rotation = msg->pose.pose.orientation;
        tf_broadcaster_->sendTransform(transform);
        msg->header.stamp = now;
        msg->pose.pose.position.z = 0.0; 
        odom_pub_->publish(*msg);
    }

    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        msg->header.stamp = this->now();
        imu_pub_->publish(*msg);
    }

    void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        rclcpp::Time now = this->now();
        rclcpp::Time stamp_with_margin = now - rclcpp::Duration(0, 20000000); 
        
        msg->header.stamp = stamp_with_margin;
        pointcloud_pub_->publish(*msg);
    }

    void publish_static_tfs_from_urdf()
    {
        std::vector<geometry_msgs::msg::TransformStamped> static_transforms;
        geometry_msgs::msg::TransformStamped t;
        tf2::Quaternion q;
        t.header.stamp = this->now();
        t.header.frame_id = "base_link";
        t.child_frame_id = "dog_imu_link";
        t.transform.translation.x = 0.0;
        t.transform.translation.y = -0.02341;
        t.transform.translation.z = 0.04927;
        q.setRPY(0.0, 0.0, 0.0);
        t.transform.rotation = tf2::toMsg(q);
        static_transforms.push_back(t);
        t.header.frame_id = "base_link";
        t.child_frame_id = "rslidar";
        t.transform.translation.x = 0.34218;
        t.transform.translation.y = 0.0;
        t.transform.translation.z = 0.17851;
        q.setRPY(0.0, 0.0, 0.0);
        t.transform.rotation = tf2::toMsg(q);
        static_transforms.push_back(t);
        static_tf_broadcaster_->sendTransform(static_transforms);
        RCLCPP_INFO(this->get_logger(), "已根据 URDF 发布了 %zu 个静态TF.", static_transforms.size());
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<DataSynchronizerAndTfPublisher>("data_sync_and_tf_node");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
